COMMENT ⊗   VALID 00010 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	.TITLE DIAG
C00005 00003	PROGRAM INITIALIZATION 
C00007 00004	SETUP COEFFICIENT DATA LIST
C00012 00005	ROUTINE TO DISPLAY CURRENT JOINT ANGLES
C00016 00006	ROUTINE TO DISPLAY CURRENT REFERENCE POWER SUPPLY READINGS
C00018 00007	ROUTINE TO CHECK THE TOUCH SENSOR ON MONITOR SUBROUTINES 
C00019 00008	CENTER FUNCTION TEST ROUTINE
C00021 00009	DRIVE COEFFICIENT DATA LIST
C00023 00010	LOCAL STORAGE AREA
C00025 ENDMK
C⊗;
.TITLE DIAG

TRANS ==1	;SET TO 1 TO PRINT OUT EULER AS WELL AS JOINT ANGLES

SNGSTP==0	;CONDITIONAL ASSEMBLY FLAGS FOR ARM.PAL
DIAGY ==1
STDALN==1
TACCAL==0
NOYELW==1
TIMER ==0
ISLIN ==1
FRCDAT==0

.INSRT K1DEF.PAL[11,SYS]
.INSRT ALHEAD.PAL[AL,HE]
.INSRT ARM.PAL[AL,HE]
.INSRT BEJCZY.PAL[AL,HE]
.INSRT ARITH.PAL[AL,HE]
.INSRT IO.PAL[3,BES]
.IFNZ TRANS
   .INSRT ARMSOL.PAL[11,BES]
   .INSRT EULER.PAL[AL,HE]
.IFF
   UPDATE:
   SOLVE:	0
.ENDC

;COMMUNICATION LINK FOR "DIAGY"

CMDARR==110000	;ADDRESS OF COMMAND INPUT BUFFER FROM PDP10
JOINT ==110002	;JOINT NUMBER TO BE DRIVEN
TIME  ==110004	;DURATION OF DRIVE MOTION
JTANG ==110006	;CHANGE IN JOINT ANGLE, F.P.
MASLOC==110012	;ADDRESS OF MASTER CHECK NUMBER FROM PDP10
DSTAT ==110014	;FUNCTION STATUS

MASTER==13131	;CHECK NUMBER FROM PDP10


;COMMUNICATION LINK FOR "TLKEF5", "WHERE" ROUTINE FOR THE PDP10

MASLC5==110100	;ADDRESS OF MASTER NUMBER TO SIGNAL PDP10 WE ARE ALIVE
TRNADR==110102	;SAVE TRANSFORM DATA IN HERE
JTSADR==110202	;SAVE JOINT ANGLES IN HERE

MASTR5==12121	;CHECK NUMBER TO PDP10


;DEVICE MECHANISM BITS

YELARM== 1	;MECHANISM BITS: YELLOW ARM, NOT INCLUDING HAND
YELHND== 2 	;YELLOW HAND
BLUARM== 4	;BLUE ARM, NOT INCLUDING HAND
BLUHND== 10	;BLUE HAND
;PROGRAM INITIALIZATION 

.EVEN
DIAG:	MOV 	#START,JOBSA
	JMP	1000

START:	MOV	#DEVICE,R1	;INITIALIZE BLUE ARM FOR MOVING
	JSR	PC,INTARM
	TST	R0		;CHECK IF ANY ERRORS OCCURRED
	BEQ	STRTOK		;BRANCH IF EVERYTHING OK
BADINT:	MOV	#BADSTR,SG	;ELSE TYPE ERROR MESSAGE
TYPERR:	JSR	PC,TYPSTR
	MOV	#IOBUF,SG	;TYPE ERROR CODE
	JSR	PC,CVO
	MOV	#IOBUF,SG
	JSR	PC,TYPSTR
	JSR	PC,CRLF
	DISMIS
STRTOK:	CLR	MASLOC		;CLEAR MASTER NUMBER FROM PDP10
     	MOV 	#-1,CMDARR	;INDICATE READY TO RECEIVE COMMAND BLOCK FROM 10

;WAIT LOOP LOOKING AT COMMAND BLOCK FROM PDP10

WTLP:   TST	KBIS		;CHECK IF ANYONE HIT VT05 KEYBOARD
	BEQ	CHKMST
	CLRB	KBIR
	CLR	CMDARR		;INDICATE DIAG NO LONGER ACTIVE
	DISMIS			;EXIT TO DDT
CHKMST:	TST	MASLOC		;CHECK IF COMMAND BLOCK RECEIVED FROM PDP10
	BEQ	WTLP		;GO LOOK AGAIN IF ZERO    
	CMP	#MASTER,MASLOC	;CHECK IF RIGHT MASTER NUMBER FROM PDP10
	BEQ	SMENUM
	MOV	#-2,CMDARR	;SET ERROR CODE INDICATING BAD NUMBER
	CLR	MASLOC		;CLEAR THE BAD MASTER NUMBER
	JMP	WTLP		;GO WAIT FOR ANOTHER COMMAND BLOCK
SMENUM:	CLR	MASLOC 

;SETUP COEFFICIENT DATA LIST

	MOV	#100000,R0	;THIS IS YELLOW ARM JOINT 1 SERVO BIT
	TST	CMDARR     	;GET THE ARM NUMBER, EQUAL TO 1 IF BLUE ARM
	BEQ	.+6   		;SKIP IF YELLOW
	MOV	#400,R0		;THIS IS BLUE ARM JOINT 1 SERVO BIT
	MOV	JOINT,R1	;GET THE JOINT NUMBER 1-7
	DEC	R1     
       	NEG	R1
	ASH	R1,R0		;SHIFT BIT TO INDICATE PROPER JOINT 
       	MOV	R0,DLST		;SET SERVO BIT
	MOV	TIME,DTIME	;SET DURATION OF DRIVE MOTION
	MOV	#DA0,R0		;SET A0,A1,A2 EQUAL TO ZERO
	MOV	#6,R1
	CLR	(R0)+
	SOB	R1,.-2
	LDF	JTANG,AC0	;GET THE CHANGE IN JOINT ANGLE
	LDF	C10,AC1		;SCALE 5TH ORDER POLYNOMIAL BY CHANGE AND PACK
	MULF	AC0,AC1		;  IN TO COEFFICIENT DATA LIST
	STF	AC1,DA3
	LDF	CM15,AC1
	MULF	AC0,AC1
	STF	AC1,DA4
	LDF	C6,AC1
	MULF	AC0,AC1
	STF	AC1,DA5

;SET UP THE DYNAMIC COEFFICIENT LIST CI AND CII

	BIT	#YARM+YHAND,DLST	;CHECK IF THIS IS FOR THE YELLOW ARM
	BEQ	NOYEL
	MOV	#YARM+YHAND,WLST	;IF SO, READ THE CURRENT JOINT ANGLES
	MOV	#YELARM,R2	;INDICATE YELLOW ARM
	MOV	#YTH,R4		;GET POINTER TO YELLOW JOINT ANGLES
	BR	FILLAN
NOYEL:	MOV	#BARM+BHAND,WLST	;ELSE MUST BE BLUE ARM
	MOV	#BLUARM,R2	;INDICATE BLUE ARM
	MOV	#BTH,R4		;GET POINTER TO BLUE JOINT ANGLES
FILLAN:	MOV	#WLST,R0
	MOV	#DEVICE,R1
	JSR	PC,WHERE
	BIT	#YHAND+BHAND,DLST	;CHECK IF HAND SELECTED FOR DRIVE
	BNE	ISHND		;BRANCH IF HAND OPERATION
	MOV	JOINT,R3	;GET POINTER TO JOINT ANGLE
	DEC	R3
	ASH	#1,R3
	ADD	R4,R3
	LDF	@(R3),AC0	;GET CURRENT JOINT ANGLE
	ADDF	JTANG,AC0	;COMPUTE FINAL JOINT ANGLE
	STF	AC0,@(R3)
	MOV	R4,R0		;COMPUTE CORRESPONDING ARM DYNAMIC COEFFICIENTS
	MOV	#CIPTR,R1
	JSR	PC,DTERMS
	MOV	JOINT,R3	;GET INDEX TO DYNAMIC COEFFICIENTS
	ASH	#2,R3
	LDF	CILST-4(R3),AC0	;PICK UP GRAVITY LOADING AND INERTIA
	STF	AC0,SEGCI
	LDF	CIILST-4(R3),AC0
	STF	AC0,SEGCII
       	MOV	#WLST,R0	;RESTORE CURRENT JOINT ANGLES
	MOV	#DEVICE,R1
	JSR	PC,WHERE
	BR	DODRVE
ISHND:	BIT	#YHAND,DLST	;CHECK IF YELLOW HAND
	BEQ	NOYHND
	LDF	CI+SRV07,AC0	;IF YELLOW GET PROPER CI AND CII
	LDF	CII+SRV07,AC1
	BR	DOHAND
NOYHND:	LDF	CI+SRV14,AC0	;THEN IT MUST BE THE BLUE HAND
	LDF	CII+SRV14,AC1
DOHAND:	STF	AC0,SEGCI	;PUT DYNAMIC COEF. IN DATA LIST
	STF	AC1,SEGCII

;START UP ARM MOTION USING DRIVE FUNCTION

DODRVE: MOV	#DLST,R0	;POINT TO COEFFICIENTS
	MOV	#DEVICE,R1	;POINT TO DEVICE BLOCK
	JSR	PC,DRIVE
	MOV	R0,DSTAT	;SAVE FUNCTION STATUS

;INDICATE EXECUTION COMPLETED TO PDP10

RET:	MOV	DBUF,R0      	;SET DBUF EQUAL TO THE NUMBER OF DATA BLOCKS
	SUB	#DBUF+2,R0	;   COLLECTED
	ASR	R0
	MOV	R0,DBUF
     	MOV	#DBUF,R0	;GET THE ADDRESS OF THE DYNAMIC RESPONSE DATA
	ASR	R0		;GET WORD ADDRESS
	NEG	R0		;COMPLEMENT TO USE AS FLAG TO TELL PDP10 WERE DONE
	MOV	R0,CMDARR
	JMP	WTLP		;GO LOOP WAITING FOR ANOTHER COMMAND BLOCK


;END OF EXECUTABLE CODE
;ROUTINE TO DISPLAY CURRENT JOINT ANGLES

W:	MOV	#START2,JOBSA
	JMP	1000

START2:	MOV	#BARM+BHAND,WLST	;READ ALL BLUE ARM JOINTS
	MOV	#HEADER,SG	;TYPE OUT COLUMN HEADER
	JSR	PC,TYPSTR
	MOV	#8.,R0
	MOV	#2,R1
	JSR	PC,FORMAT
REDLP: 	MOV	#WLST,R0	;POINT TO WHERE COEF. LIST
	MOV	#DEVICE,R1	;POINT TO DEVICE BLOCK
	JSR	PC,WHERE	;READ THE CURRENT JOINT ANGLES
	TST	R0		;CHECK FOR ERRORS
	BEQ	GOODW		;BRANCH IF OK
	MOV	#BADW,SG	;ELSE TYPE ERROR MESSAGE
	CLR	MASLC5		;AND INDICATE TO PDP10 THAT WE ARE DEAD
	JMP	TYPERR
GOODW:	MOV	#COM4,SG	;POSITION CURSOR
	JSR	PC,TYPSTR
      	MOV	#IOBUF,SG	;START BUILDING OUTPUT STRING IN HERE
	MOV	#BTH,R2		;GET POINTERS JOINT READINGS
	MOV	#7,R3		;7 JOINTS IN ALL
JTCLP:	LDF 	@(R2),AC0	;GET JOINT ANGLE
	TST	(R2)+		;POINT TO NEXT JOINT
	JSR	PC,CVF		;CONVERT JOINT ANGLE TO STRING 
	SOB	R3,JTCLP
	MOVB	#15,(SG)+	;PUT IN A CR LF
	MOVB	#12,(SG)+
	MOV	#9.,R0		;CHANGE STRING FORMAT FOR INTEGER NUMBER
	CLR	R1
	JSR	PC,FORMAT
	MOV	#BSRVOS,R1	;GET POINTERS TO SERVOS
	MOV	#7,R3		;CONVERT THE ACTUAL POT READINGS
NEXTP:	MOV	(R1)+,R0
	LDCIF	POT(R0),AC0	;GET A/D READING
	TST	POT(R0)		;CHECK IF ROUND UP OR DOWN
	BLT	RNDDN
	ADDF	#40000,AC0	;ROUND OFF READING
	BR	.+6
RNDDN:	SUBF	#40000,AC0
	JSR	PC,CVF		;CONVERT TO ASC
	SOB	R3,NEXTP
	JSR	PC,RSTFOR
	CLRB	(SG)
	MOV	#IOBUF,SG	;TYPE OUT WHOLE STRING
	JSR	PC,TYPSTR
   .IFNZ TRANS
	MOV	#TRNADR,R0	;GET CURRENT TRANSFORM IN HERE
	MOV	#THPTR,R1	;POINT TO THE CURRENT JOINT ANGLES
	MOV	#BLUARM,R2	;USE SOLUTION FOR BLUE ARM
	JSR	PC,UPDATE	;COMPUTE THE CURRENT TRANSFORM
	MOV	#TRNADR,R0	;COMPUTE THE EULER ANGLES
	MOV	#EANGLE,R1	;PUT THEM IN HERE
	JSR	PC,EULER
	MOV	#IOBUF,SG	;CONVERT TO STRING FOR OUTPUT
	MOVB	#15,(SG)+	;PUT IN A CR LF LF
	MOVB	#12,(SG)+
	MOVB	#12,(SG)+
	MOV	#6,R0
	MOV	#EANGLE,R1
EANGC:	LDF	(R1)+,AC0
	JSR	PC,CVF
	SOB	R0,EANGC	;REPEAT TILL DONE
	CLRB	(SG)
	MOV	#IOBUF,SG	;TYPE OUT THE EULER ANGLES
	JSR	PC,TYPSTR
	MOV	#BTH,R0		;TRANSFER THE JOINT ANGLES TO PDP10 
	MOV	#JTSADR,R1	;   COMMUNICATIONS TABLE
	MOV	#7,R2		;6 JOINTS + HAND
RHTLP:	LDF	@(R0)+,AC0	
	STF	AC0,(R1)+
	SOB	R2,RHTLP
	MOV	#MASTR5,MASLC5	;INDICATE TO PDP10 THAT "WHERE" IS WORKING
   .ENDC
        TST	KBIS		;CHECK IF ANYONE HIT VT05 KEYBOARD
	BEQ	REDLP
	CLRB	KBIR
	CLR	MASLC5		;INDICATE "WHERE" NOT ACTIVE
	DISMIS
;ROUTINE TO DISPLAY CURRENT REFERENCE POWER SUPPLY READINGS

REF:	MOV	#START3,JOBSA
	JMP	1000

START3:	MOV	#COM5,SG	;TYPE OUT THE CALIB. REFERENCE READING
	JSR	PC,TYPSTR
	MOV	REFER1,R0	;GET REF. READING
	MOV	#IOBUF,SG
	JSR	PC,CVI
	MOVB	#54,(SG)+	;PUT IN A COMMA AND SPACE CHARACTER
	MOVB	#40,(SG)+
	MOV	REFER2,R0	;GET SECOND REF. READING
	JSR	PC,CVI
	MOV	#IOBUF,SG	;TYPE THEM OUT
	JSR	PC,TYPSTR
	JSR	PC,CRLF
	CLR	DEVICE		;DON'T READ ANY TACHS
REFLP:	MOV	#DEVICE,R1	;READ THE REF. POWER SUPPLY VOLTAGE
	JSR	PC,SETREF
      	MOV	#REFSG2,SG	;START BUILDING OUTPUT STRING IN HERE
	MOV	REFVT1,R0	;GET REFERENCE READINGS AND CONVERT TO ASC
	JSR	PC,CVI
	MOVB	#54,(SG)+	;PUT IN A COMMA AND SPACE CHARACTER
	MOVB	#40,(SG)+
	MOV	REFVT2,R0	;GET THE SECOND READING
	JSR	PC,CVI
	MOVB	#40,(SG)+	;PUT IN A FEW SPACE CHARACTERS
	MOVB	#40,(SG)+
	MOVB	#40,(SG)+
	MOV	#REFSTG,SG	;TYPE OUT WHOLE STRING
	JSR	PC,TYPSTR
        TST	KBIS		;CHECK IF ANYONE HIT VT05 KEYBOARD
	BEQ	REFLP
	CLRB	KBIR
	JSR	PC,CRLF
	DISMIS
;ROUTINE TO CHECK THE TOUCH SENSOR ON MONITOR SUBROUTINES 

TTOUCH:	MOV	#START5,JOBSA
	JMP	1000

START5:	EVMAK			;CREATE A EVENT TO WAIT ON
	MOV	(SP),R0		;GET THE EVENT NUMBER
	MOV	SENSOR,R1	;GET THE SENSOR NUMBER AND TRIGGER STATE
	JSR	PC,TOUCH	;INITIATE TOUCH SENSOR ON MONITOR
	EVWAIT	(SP)		;WAIT FOR EVENT TO BE TRIGGERED
	EVKIL			;GET RID OF THE EVENT
	MOV	#EVMES,SG	;TELL EVERYONE THE EVENT HAS BEEN TRIGGERED
	JSR	PC,TYPSTR
	DISMIS			;EXIT TO THE DEBUGGER

;CENTER FUNCTION TEST ROUTINE

C:     	MOV	#START6,JOBSA
	JMP	1000

START6:	MOV	#DEVICE,R1	;INITIALIZE BLUE ARM TO RUN
	JSR	PC,INTARM
	TST	R0		;CHECK FOR ERRORS
	BEQ	.+6		;SKIP IF NO ERRORS
	JMP	BADINT		;BRANCH IF INITIALIZATION ERROR
        MOV	#BARM+BHAND,DLST	;CENTER BLUE ARM
        MOV	#DLST,R0	;POINT TO COEFFICIENTS
	MOV	#DEVICE,R1	;POINT TO DEVICE BLOCK
	JSR	PC,CENTER
	MOV	#COM6,SG	;TYPE OUT STATUS WORD
	JSR	PC,TYPSTR
	MOV	#IOBUF,SG
	JSR	PC,CVO
	MOV	#IOBUF,SG
	JSR	PC,TYPSTR
	JSR	PC,CRLF
	MOV	DEVICE,R2	;GET THE NUMBER OF  SERVOS ATTACHED
	BEQ	ENDS5		;BRANCH IF NONE ATTACHED
	MOV	#COM7,SG	;ELSE TYPE OUT THE DEVICE BLOCK CONTENTS
	JSR	PC,TYPSTR
	MOV	#DEVICE+4,R1	;START WITH THIS WORD
DEVLP2:	MOV	(R1)+,R0
	MOV	#IOBUF,SG
	JSR	PC,CVO
	MOV	#IOBUF,SG
	JSR	PC,TYPSTR
	JSR	PC,CRLF
	SOB	R2,DEVLP2
ENDS5:	DISMIS			;EXIT TO THE DEBUGGER

;END OF CENTER AND TOUCH SENSOR TESTS
;DRIVE COEFFICIENT DATA LIST

DLST:	0       ;JOINT SERVO BIT
	0
	0	;NO SPECIAL MODES
	0	;NO WOBBLE
	40.	;RELATIVE SEGMENT POINTER
DTIME:	160.
	0	;NO ASSOCIATED TRANS
	0	;NO RUN CODE
DA0:	.WORD	0,0
	.WORD	0,0
	.WORD	0,0
DA3:	.WORD	41040,0		;10.0
DA4:	.WORD 	141160,0	;-15.0
DA5:	.WORD	40700,0		;6.0
SEGCI:	.BLKW	2		;CI
SEGCII:	.BLKW	2		;CII
	0	;NO OTHER SEGMENTS

;WHERE COEFFICIENT DATA LIST

WLST:	374     ;JOINT SERVO BIT
	0

;FLOATING POINT CONSTANTS

C10:	.WORD	41040,0		;10.0
CM15:	.WORD	141160,0	;-15.0
C6:	.WORD	40700,0		;6.0

;TABLES FOR COMPUTE DYNAMIC COEFFICIENTS

CIPTR:	CILST
	CILST+4
	CILST+10
	CILST+14
	CILST+20
	CILST+24
	CIILST
	CIILST+4
	CIILST+10
	CIILST+14
	CIILST+20
	CIILST+24
CILST:	.BLKW	12.
CIILST:	.BLKW	12.
;LOCAL STORAGE AREA

DEVICE:	.BLKW	32	;DEVICE BLOCK
IOBUF:	.BLKW	400.
EANGLE:	.BLKW	12.
SENSOR:	.BYTE	0,1	;BLUE ARM TOUCH SENSOR, TRIGGER ON ON CONDITION

;OUTPUT STRINGS

COM4:	.BYTE	15,32,10,10,10
   .IFNZ TRANS
	.BYTE	32,10,10,10,32,10,10,10
   .ENDC
	.BYTE	0
HEADER:	.ASCII	/
   JT 1    JT 2    JT 3    JT 4    JT 5    JT 6     HAND

/
   .IFNZ TRANS
        .ASCII  /
     X       Y       Z       O       A       T
/
   .ENDC
	.BYTE	0
BADW:	.ASCIZ	/
SERVO ERROR WHILE ATTEMPTING TO DO A "WHERE", ERROR CODE = /
BADSTR:	.ASCIZ	/
ARM INITIALIZATION FAILED, ERROR CODE = /
COM5:	.ASCIZ	/
REF. POWER SUPPLY READINGS DURING CALIBRATION = /
REFSTG:	.ASCII	/CURRENT REFERENCE READINGS = /
REFSG2:	.ASCII	/                /
	.BYTE	15,10,10,10,0
COM6:	.ASCIZ	/STATUS WORD = /
COM7:	.ASCIZ	/THE DEVICE BLOCK SERVO WORDS ARE AS FOLLOWS:
/
EVMES:	.ASCII	/TOUCH SENSOR EVENT TRIGGERED
/
	.BYTE	7,0
.EVEN

;MAIN PROGRAM PROCESSOR DESCRIPTOR BLOCK

MAINL:	PDBLK	1,100,FP
	
TEMP == .		
. = JOBDAT		;MAIN PROGRAM DESCRIPTOR BLOCK
	MAINL        
. = JOBSA		;START OF MAIN PROGRAM
	START2
. = TEMP		;RESTORE LOCATION COUNTER

.END DIAG